#ifndef _RIGID_BODY_H_
#define _RIGID_BODY_H_

#include <vec.h>
#include <mat.h>
#include <vector>
#include "quaternion.h"
#include "Polyhedron.h"
#include "SDM.h"
#include <string>

using namespace std;

typedef Quaternion<double> Quaterniond;

extern class CollisionPair;
extern class RigidBody;

class State
{
public:
	State(void){};
	~State(void){};
	void setState(RigidBody* b);

	Vec3d pos;
	Vec3d vel;
	Quaterniond orientation;
	Vec3d rotation;
};

class RigidBody
{
public:
	RigidBody(void);
	~RigidBody(void);
	RigidBody(string filename, double density, double res);

	void initRigidBody(initializeFunc setUp);


	void rebuildMatrices();


	Vec3d getWorldVelocity(Vec3d x);
	Vec3d getPointWorldToBody(Vec3d p);
	Vec3d getPointBodyToWorld(Vec3d p);
	Vec3d getVectorWorldToBody(Vec3d v);
	Vec3d getVectorBodyToWorld(Vec3d v);


	void setState(State src);


	Value evaluateSDM(Vec3d p, bool compute_normal);


	Mat33d computeK(Vec3d x);


	void applyImpulse(Vec3d x, Vec3d J);
	void applyForce(Vec3d x, Vec3d f);
	void resetForce();
	void updatePos(double dt);
	void updateVel(double dt);


	void computeMassProperty();
	double getDeterminant(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3);
	void addTriangleContribution(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3);


	double bounciness;
	double friction;
	double mass;	
	double volume;
	double density;
	double radius;	
	double res;

	Polyhedron* poly;
	SDM* sdm;

	Vec3d pos;	
	Quaterniond orientation;

	Vec3d netForce;
	Vec3d netTorque;

	Vec3d vel;
	Vec3d rotation;
	
	Mat33d orientationMatrix;
	Mat33d inverseOrientationMatrix;

	Mat33d bodyInertiaMatrix;
	Mat33d inverseWorldInertiaMatrix;	
	Mat33d inverseInertiaTensor;

	Mat33d inverseMassMatrix;

	vector<CollisionPair*>* collisionGraph;

	double centerX;
	double centerY;
	double centerZ;

	double inertiaTensorAtRest[9];

	bool frozen;

};

typedef vector<RigidBody*> RigidBodyList;

#endif



